#include "stm32f10x.h"                  // Device header
#include "Delay.h"
#include "OLED.h"
#include "Encoder_Interface.h"
#include "TIM.h"
#include "PWM.h"
#include "Uart.h"
#include "DMA_UART1.h"
#include <stdio.h>
#include "Rp.h"
#include "Key.h"
#include "I2C.h"



static char buf[20];

float target_speed ;
float current_rps;
uint16_t output_duty ;

uint16_t current_encoder_count ;
float Kp,Ki,Kd;

int main(void)
{
    
    RCC_HSEConfig(RCC_HSE_ON);
    
    while(RCC_WaitForHSEStartUp()==ERROR);
    
    RCC_PLLConfig(RCC_PLLSource_HSE_Div1,RCC_PLLMul_9);
    
    RCC_PLLCmd(ENABLE);
    
    RCC_SYSCLKConfig(RCC_SYSCLKSource_PLLCLK);
    
 
    
	/*模块初始化*/
    

	
    Key_Init();

    
    init_uart_with_dma();
    
    init_dma_for_uart1();
    

    I2C1_Init();
    
    OLED_Init();//OLED初始化
    
    
    init_tim();
    
    

    init_encoder_interface_in_tim3();
    
    init_my_pwm_in_tim2();
    
    start_pwm();
    
    initRp();
    
   
	while (1)
	{
        
        sprintf(buf,"K:%2.1f-%2.1f-%2.1f", Kp,Ki,Kd);
        OLED_ShowString(1,1,buf);
        
        sprintf(buf,"cur_enc:%d", current_encoder_count);
        OLED_ShowString(2,1,buf);
        
        sprintf(buf,"tar:%3.1f", target_speed);
        
        OLED_ShowString(3,1,buf);
            
        sprintf(buf,"cur:%3.1f-%03d",current_rps, output_duty);
        
        OLED_ShowString(4,1,buf);
        
	}
}
/**
    adc 的 dma 传输完成中断已被关闭
*/
void DMA1_Channel1_IRQHandler(void){
    if(DMA_GetITStatus(DMA1_IT_TC1)==SET){
        /*
        static char buf[20];
        
        
        sprintf(buf,"rp1:%d %d", get_rp_val(0),k1);
        OLED_ShowString(1,1,buf);
        
        sprintf(buf,"rp2:%d %d", get_rp_val(1),k2);
        OLED_ShowString(2,1,buf);
        
        sprintf(buf,"rp3:%d %d", get_rp_val(2),k3);
        OLED_ShowString(3,1,buf);
        
        sprintf(buf,"rp4:%d %d", get_rp_val(3),k4);
        OLED_ShowString(4,1,buf);
        */
        //uint16_t duty = get_rp_val(0)*2.44;//*10000/4096;
        
        //TIM_SetCompare1(TIM2,duty);
        
        DMA_ClearITPendingBit(DMA1_IT_TC1);
    }
}

uint16_t k1=1,k2=0,k3=0,k4=0;

void EXTI15_10_IRQHandler(void){
    
    if(EXTI_GetITStatus(EXTI_Line10)==SET){
        if(GPIO_ReadInputDataBit(GPIOB,GPIO_Pin_10)==Bit_RESET){
            //K1 被按下
            if(k1==0){
                k1 = 1;
                start_pwm();
            }else{
                k1 = 0;
                stop_pwm();
            }
        }
        EXTI_ClearITPendingBit(EXTI_Line10);
    }else if(EXTI_GetITStatus(EXTI_Line11)==SET){
        if(GPIO_ReadInputDataBit(GPIOB,GPIO_Pin_11)==Bit_RESET){
            //K2 被按下
            k2++;
        }else if(GPIO_ReadInputDataBit(GPIOA,GPIO_Pin_11)==Bit_RESET){
            //K3 被按下
            k3++;
        }
        
        EXTI_ClearITPendingBit(EXTI_Line11);
    }
    else if(EXTI_GetITStatus(EXTI_Line12)==SET){
        if(GPIO_ReadInputDataBit(GPIOA,GPIO_Pin_12)==Bit_RESET){
            //K4 被按下
            k4++;
        }
        EXTI_ClearITPendingBit(EXTI_Line12);
    }
    
}

#define MAX_RPS 23
const float pulse_per_rotation = 4 * 11 *9.27666; // 脉冲每转

static float error_sum = 0;
//static float last_error = 0;


/**
    PID 三个参数从某种意义上分别代表了调控的快(p比例)、准(i积分)、稳(d微分)
    
    位置式PID
        缺点：
            1、积分饱和：积分项的累加求和如果在Ki处于0时，误差会被不断的累加，
                当某次调参时，设置Ki>0时，会有一个很明显的抖动
                解决：当Ki==0时，对积分球和不进行累加
    增量式PID
        优点：
            1、不存在位置式PID的积分饱和问题
                因为out项的《输出限幅》 兼具《积分限幅》的功能
            2、把最终pid函数的数据结果out直接简单粗暴的限幅到电机输出的范围，不用考虑其他
            3、方便自动控制和手动控制平滑切换
                把Kp、Ki、Kd全都设置为0，手动修改out值
        缺点:
            Kp一项，具体是Kp(error0 - error1)  会引入历史信息
            如果每次存在微小的error变化，那么Kp项很难自动调节，因为依赖了error1，甚至有积分项的特性
            加上Ki项才能缓解这个问题
            所以增量式PID调节，不建议Ki项为0
            
*/
float pid(float error,float Kp,float Ki,float Kd,float dt){
    
   
    /*
    二、积分分离（应用于位置式pid控制电机位置场景）
        PD 控制器的缺点：
            1、target 与 current 之间总是存在误差无法消除
                这是因为误差过小导致Kp项无法对其进行修正
            2、用手小幅拨动转盘，发现能轻易改变电机位置（电机输出太小）
        引入积分项（Ki）后能解决问题一，但是会额外引入下列问题：
            1、由于电机定位控制在out达到target时，
                不需要额外的 积分项（Ki）来抵抗摩檫力，
                所以会导致积分项（Ki）输出之前累积过大值，出现过调
        解决方案：当误差值大于阈值的时候关闭Ki，反之则开启Ki
    */
    
    /*
        使用微分先行，对输出（当前值）加阻尼
    */
    static float last_rps = 0;
    
    /*
    一般不建议乘以dt 或者 除以dt 
    这会使得Kp Ki Kd 不在一个量级(因为Kp项与dt无关)，
    调参的时候不方便
    所以后续的dt传入进来的都是 1
    */
    float result = Kp * error 
        +  Ki * error_sum * dt
        -  Kd * (current_rps - last_rps) /dt;
    
    error_sum += error;
    
    /*
    一、积分限幅
    这里的积分上限设置为：
    当pid输出最大 rps ，且只有 Ki项不为0 时
    积分的和
    */
    float max_sum =  Ki * error_sum;
    
    if(error_sum > 200){
        /**
        为解决位置式pid 积分饱和问题
        这里采用积分限幅功能
        幅值 200 由观察上位机一般情况下最大的 error_sum 来确定
        */
        error_sum = 200;
    }else if(error_sum < -200){
        error_sum = -200;
    }
    
    //last_error = error;
    
    last_rps = current_rps;
    
    return result;
}






/**
输入目标速度，返回当前速度
*/
float inner_pid(float target_rps,uint16_t current_encoder_count,float Kp,float Ki,float Kd,float dt){
    
    static uint16_t lastCount = 0;
    

    /*
        定时越快，读取值的频率越快，但是测量精度会下降
        定时越慢，测量精度越高，但是获取值的频率就必须降下来
    
        注意这里实现正反转的话，涉及到的变量类型必须是有符号的
    */
    int16_t delta_count = current_encoder_count - lastCount;
    
    
    // 因为这个方法40ms触发一次，所以要乘以25Hz得到转/秒
    // 1/0.04 = 25
    current_rps = delta_count * 25 / pulse_per_rotation;
    
    
    // 当前比较好的参数 p=1.7 i=3.5 d=?
    float error = target_rps - current_rps;
    
    float output_rps = pid(error,Kp,Ki,Kd,dt);
    
    output_duty = set_speed(output_rps,MAX_RPS);
    

        
    lastCount = current_encoder_count;

    return current_rps;
}

int16_t encoder_to_signed(uint16_t raw) {
    return (int16_t)(raw - 32768); // 32768→0, 65535→32767, 0→-32768
}

int16_t current_location;
int16_t error_location;
/**
位置的单位采用 正负 360°
*/
float location_pid(int16_t target_location,uint16_t current_encoder_count,float Kp,float Ki,float Kd,float dt){

    static float error_location_sum = 0;
    //static uint16_t last_encoder_count = 0;
    static int16_t last_location = 0;
    


   
    //int16_t delta_encoder_count = current_encoder_count - last_encoder_count;
    /*
    因为stm32标准库编码器cnt获取得到的是 uint16_t 返回是0~65535
    电机正向旋转没有问题，但是如果反向旋转
    那么会从0突然跳变为65535，从而引发一系列的系统不稳定问题
    
    所以这里将uint16_t 强制转换为 int16_t （范围-32768~32767）
    这样在编码器数值没有溢出（没有超过 正负32768）的时候，系统表现正常
    */
    current_location = (int16_t)(current_encoder_count) * 360 /pulse_per_rotation ;
    
    error_location = target_location - current_location;
    
    error_location_sum += error_location;
    
    float location_out = Kp * error_location 
        + Ki * error_location_sum * dt
        + Kd * ( current_location - last_location )/ dt;
    

    
   
    
    //last_encoder_count = current_encoder_count;
    last_location = current_location;
    
    return location_out;
}

const float dt = 0.04;

void TIM4_IRQHandler(void){
    
    if(TIM_GetITStatus(TIM4,TIM_IT_Update)){
        
        // 这里应该改为读取之后将其清零，这样就能每次都读取编码器的增量
        current_encoder_count = TIM_GetCounter(TIM3);
    

        
        // 电机目标位置：范围 -360° ~ +360°
        int16_t target_location = get_rp_val(0) * 2 * 360  / 4096  - 360;

       

        Kp = get_rp_val(1)/4096.0 * 2.5;     
        Ki = get_rp_val(2)/4096.0 * 2;
        Kd = get_rp_val(3)/4096.0 * 2;
        
        
    
        
        float location_out = location_pid(target_location,current_encoder_count,
            Kp, //0.48
            Ki,
            Kd, //0.33
        1);
        
        // 外环PID的输出值作用于内环PID的输入值（目标值） 
        target_speed = location_out * MAX_RPS /360; //TODO
        
        
        
        /* 电机目标速度：范围 -20 rps ~ +20 rps
        target_speed = get_rp_val(0) * 2 * 20  / 4096  - 20;
            改变目标速度的最大最小值可以限制输出的速度
        */
        if (target_speed > MAX_RPS){
            target_speed = MAX_RPS;
        }else if (target_speed < -MAX_RPS){
            target_speed = -MAX_RPS;
        }
        
        inner_pid(target_speed,current_encoder_count,1.7,0.7,0.27,1);

        
        //sprintf(buf,"tar:%03.1f-cur:%.1f", current_rps,Ki);
        //OLED_ShowString(2,1,buf);
        
    
        
        uint8_t * str  = getBuffer();

    
        int length = sprintf((char *)str,"%d,%d,%f,%f,%03.2f,%03.2f,%03.2f,%d\r\n",
            
            target_location,
            current_location,
            location_out,
            target_speed,
            Kp*10,Ki*10,Kd*10,
            error_location
        );  
        
       
        
        send(length);
        
        TIM_ClearITPendingBit(TIM4,TIM_IT_Update);
    }
}

